#include "plugins/action/find_lr_zerocross.hpp"

BT::NodeStatus FindLrZeroCross::onStart()  {
    std::cout << "FindLrZeroCross_onStart()" << std::endl;
    if (!getInput<uint64_t>("T_to", T_to_)) {
        throw BT::RuntimeError("Missing required input 'FindLrZeroCross.T_to'");
    }
    if (!getInput<std::shared_ptr<std::vector<uint64_t>> >("mean_swing_duration", mean_swing_duration_ptr)) {
        throw BT::RuntimeError("Missing required input 'FindLrZeroCross.mean_swing_duration'");
    }
    //从端口获取mean_swing_duration数组的数据
    if (mean_swing_duration_ptr) {
        mean_swing_duration_ = *mean_swing_duration_ptr;
    } else {
        std::cout << "mean_swing_duration_ptr is null!" << std::endl;
    }
    print_all_inports(true);


    cnt = 0;
    tmp_sensor_data_filtered_={};
    this->onRunning();
    return BT::NodeStatus::RUNNING;
}


BT::NodeStatus FindLrZeroCross::onRunning(){
    /*从端口获取数据*/
    if (!getInput<sensorData>("sensor_input", tmp_sensor_data_filtered_)) {
        throw BT::RuntimeError("Missing required input 'FindLrZeroCross.sensor_input'");
    }

    // cnt++; //调试逻辑用
    // if (cnt > 3)
    // {
    //     std::cout << "GripperOpen_SUCCESS" << std::endl;
    //     return BT::NodeStatus::SUCCESS;
    // }
    //当前时间ms
    // auto now = std::chrono::steady_clock::now();
    // auto duration = now.time_since_epoch();
    // current_millis_ = std::chrono::duration_cast<std::chrono::milliseconds>(
    //     std::chrono::steady_clock::now().time_since_epoch()).count();

    current_millis_ = config().blackboard->get<uint64_t>("global_counter");
    current_millis_ *= 2;
    // std::cout<<"FindLrZeroCross_current_millis_"<<current_millis_ <<std::endl;

    // 调试日志
    // std::cout << "Current time (ms): " << current_millis_ 
    //         << ", T_to: " << T_to_ 
    //         << ", diff: " << (current_millis_ - T_to_) << std::endl;
    
    // return BT::NodeStatus::FAILURE;
        
    tof_distance_ = tmp_sensor_data_filtered_.tof_distance;

    // 获取数组mean_swing_duration_最后一个数值
    try {
        // 可能抛出异常的代码（如访问 vector 的 back() 前检查 empty()）
        if (mean_swing_duration_.empty()) {
            std::cerr << std::string(COLOR_YELLOW) + "mean_swing_duration_ Vector is empty!" + std::string(COLOR_RESET) << std::endl;
            return BT::NodeStatus::FAILURE;
        }
        last_mean_swing_duration_ = mean_swing_duration_.back();
    } catch (const std::exception& e) {
        std::cerr << "Error: " << e.what() << std::endl;
        return BT::NodeStatus::FAILURE;
    }
    // 将变量写入黑板
    config().blackboard->set("mean_swing_duration_inlrzerocross", last_mean_swing_duration_);
    if (current_millis_-T_to_ > 0.4*last_mean_swing_duration_){
            if (tof_distance_last_ > 0 && tof_distance_ <= 0)//tof过零点，由正变到负
            {
                T_fid_ = current_millis_;
                //当前时刻点
                setOutput("T_fid", T_fid_);
                this->print_all_outports(true);
                std::cout<<"FindLrZeroCross_onSUCCESS"<<std::endl;
                return BT::NodeStatus::SUCCESS;
            }
        // std::cout<<"FindLrZeroCross current_millis_-T_to_ > 0.4*last_mean_swing_duration_"<<std::endl;
        tof_distance_last_ = tof_distance_;
        return BT::NodeStatus::RUNNING;
    } else {
        tof_distance_last_ = tof_distance_;
        uint64_t tmp = current_millis_-T_to_;
        uint64_t tmp1 = static_cast<uint64_t>(0.4*last_mean_swing_duration_);
        // std::cout<<"FindLrZeroCross_current_millis_-T_to_ < 0.4*last_mean_swing_duration_"<<tmp << " righthand" << tmp1<<std::endl;
        return BT::NodeStatus::RUNNING;
    }
}

void FindLrZeroCross::print_vector(const std::vector<uint64_t>& vec, const std::string& name) {
    if (!vec.empty()) {
        std::cout << name << ": [";
        for (size_t i = 0; i < vec.size(); ++i) {
            if (i > 0) std::cout << ", ";
            std::cout << vec[i];
        }
        std::cout << "]" << std::endl;
    } else {
        std::cout << name << ": 向量为空" << std::endl;
    }
}
void FindLrZeroCross::print_var(const uint64_t& var, const std::string& name) {
    std::cout << name << ": " << var << std::endl;
}



void FindLrZeroCross::print_all_outports(bool print_switch) {
    if (!print_switch) return;
    std::cout << "=== FindLrZeroCross节点端口输出值 ===" << std::endl;
    // 打印所有变量和向量
    print_var(T_fid_, "T_fid_");
    std::cout << "======================" << std::endl;
}
void FindLrZeroCross::print_all_inports(bool print_switch) {
    if (!print_switch) return;
    std::cout << "=== FindLrZeroCross节点端口输入值 ===" << std::endl;
    // 打印所有变量和向量
    print_var(T_to_, "T_to_");
    // 打印所有向量
    print_vector(mean_swing_duration_, "mean_swing_duration_");
    std::cout << "======================" << std::endl;
}



#include "behaviortree_cpp/bt_factory.h"
BT_REGISTER_NODES(factory)
{
  std::cout << "Registering nodes...: FindLrZeroCross" << std::endl;//在宏内添加调试信息或条件判断
  factory.registerNodeType<FindLrZeroCross>("FindLrZeroCross");
}